import { Euler, Matrix4, Quaternion, Vector3 } from "../libs/three.module.js";
/* 矩阵转位置 缩放 旋转 值 */
export class MarkerToMark {

    constructor() {
        this._deltaAccuracy = 10;

        this.filterFrequency = 30.0;
        this.filterMinCutoff = 1.0;
        this.filterBeta = 0.0;
        this.filterDcutoff = 1.0;
        this._positionFilter = new OneEuroFilterVector3(this.filterFrequency);
        this._rotationFilter = new OneEuroFilterVector3(this.filterFrequency * 2);
    }

    update(world) {
        let pos = new Vector3();
        let rotationVec = new Vector3();
        let scale = new Vector3();
        if (!world) {
            this._hasFound = false;
            this._frameDrops = 0;
        } else {
            let matrixW = new Matrix4();
            let worldMatrix = matrixW.fromArray(this.getArrayMatrix(world));
            if (!this._hasFound) {
                this._hasFound = true;
                let vecTrans = new Vector3();
                this._lastTranslation = vecTrans.setFromMatrixPosition(worldMatrix);
            } else {
                let vecTrans = new Vector3();
                let _currentTranslation = vecTrans.setFromMatrixPosition(worldMatrix);
                // if (Math.abs(_currentTranslation.distanceTo(this._lastTranslation)) > this._deltaAccuracy) {
                //     this._frameDrops += 1;
                //     if (this._frameDrops > 3) {
                //         this._lastTranslation = _currentTranslation;
                //     }
                //     console.log(pos, rotationVec, scale)
                //     return [pos, rotationVec, scale];
                // }
                this._frameDrops = 0;
                this._lastTranslation = _currentTranslation;
            }
            this._positionFilter.UpdateParams(
                this.filterFrequency,
                this.filterMinCutoff,
                this.filterBeta,
                this.filterDcutoff
            );
            this._rotationFilter.UpdateParams(
                this.filterFrequency * 2,
                this.filterMinCutoff,
                this.filterBeta,
                this.filterDcutoff
            );
            let matrix = new Matrix4();

            matrix = worldMatrix;

            let rotation = new Quaternion();
            let eulerRot = new Euler();
            let position = new Vector3(0, 0, 0);

            worldMatrix.decompose(position, rotation, scale);
            let eRot = eulerRot.setFromQuaternion(rotation);
            rotationVec = this._rotationFilter.Filter(eRot.toVector3());

            pos = this._positionFilter.Filter(position);
        }
        return [pos, rotationVec, scale];
    }
    getArrayMatrix(value) {
        var array = [];
        for (var key in value) {
            array[key] = value[key];
        }
        return array;
    }
}
class OneEuroFilterVector3 {


    constructor(_freq, _mincutoff = 1, _beta = 0, _dcutoff = 1) {
        this.currValue = new Vector3();
        this.prevValue = new Vector3();

        this._freq = _freq;
        this._mincutoff = _mincutoff;
        this._beta = _beta;
        this._dcutoff = _dcutoff;

        this.oneEuroFilters = [];
        this.oneEuroFilters.push(new OneEuroFilter(_freq, _mincutoff, _beta, _dcutoff));
        this.oneEuroFilters.push(new OneEuroFilter(_freq, _mincutoff, _beta, _dcutoff));
        this.oneEuroFilters.push(new OneEuroFilter(_freq, _mincutoff, _beta, _dcutoff));
    }

    UpdateParams(_freq, _mincutoff = 1.0, _beta = 0, _dcutoff = 1) {
        this._freq = _freq;
        this._mincutoff = _mincutoff;
        this._beta = _beta;
        this._dcutoff = _dcutoff;

        for (let i = 0; i < this.oneEuroFilters.length; i++) {
            this.oneEuroFilters[i].UpdateParams(this._freq, this._mincutoff, this._beta, this._dcutoff)
        }

    }

    Filter(_value, timestamp = -1.0) {
        this.prevValue = this.currValue;

        let out = new Vector3();
        let output = out.toArray();
        let input = _value.toArray();

        this.oneEuroFilters.forEach((filters, idx) => {
            output[idx] = filters.Filter(input[idx], timestamp);
        });

        let arr = new Vector3();

        return (this.currValue = arr.fromArray(output));
    }
}
class LowPassFilter {


    constructor(alpha) {
        this.setAlpha(alpha);
        this.y = null;
        this.s = null;
    }

    setAlpha(alpha) {
        if (alpha <= 0 || alpha > 1.0) {
            throw new Error();
        }
        this.alpha = alpha;
    }

    filter(value, timestamp, alpha) {
        if (alpha) {
            this.setAlpha(alpha);
        }
        let s;
        if (!this.y) {
            s = value;
        } else {
            s = this.alpha * value + (1.0 - this.alpha) * this.s;
        }
        this.y = value;
        this.s = s;
        return s;
    }

    lastValue() {
        return this.y;
    }
}
class OneEuroFilter {

    constructor(freq, minCutOff = 1.0, beta = 0.0, dCutOff = 1.0) {
        if (freq <= 0 || minCutOff <= 0 || dCutOff <= 0) {
            throw new Error();
        }
        this.freq = freq;
        this.minCutOff = minCutOff;
        this.beta = beta;
        this.dCutOff = dCutOff;
        this.x = new LowPassFilter(this.alpha(this.minCutOff));
        this.dx = new LowPassFilter(this.alpha(this.dCutOff));
        this.lasttime = null;

        this.currValue = 0.0;
        this.prevValue = this.currValue;
    }

    alpha(cutOff) {
        const te = 1.0 / this.freq;
        const tau = 1.0 / (2 * Math.PI * cutOff);
        return 1.0 / (1.0 + tau / te);
    }

    UpdateParams(_freq, _mincutoff = 1.0, _beta = 0, _dcutoff = 1) {
        this.freq = _freq;
        this.minCutOff = _mincutoff;
        this.beta = _beta;
        this.dCutOff = _dcutoff;
        this.x.setAlpha(this.alpha(this.minCutOff));
        this.dx.setAlpha(this.alpha(this.dCutOff));
    }

    Filter(x, timestamp = null) {
        this.prevValue = this.currValue;
        if (this.lasttime && timestamp) {
            this.freq = 1.0 / (timestamp - this.lasttime);
        }
        this.lasttime = timestamp;
        const prevX = this.x.lastValue();
        const dx = !prevX ? 0.0 : (x - prevX) * this.freq;
        const edx = this.dx.filter(dx, timestamp, this.alpha(this.dCutOff));
        const cutOff = this.minCutOff + this.beta * Math.abs(edx);
        return (this.currValue = this.x.filter(x, timestamp, this.alpha(cutOff)));
    }
}